//
// Created by Shin on 2023/11/10.
//

#include "SDK.h"
#include "servo_pid.h"
//#include "my_ros.h"

int now_SDK_num = 0;
int undo_light_num = 0;
int light_num = 0;
int SDK_err = 0;

void color_figure_done(int color, int shape)
{
    //shape:1:Բ��;2:������;3:������
    //color:1:��ɫ;2:��ɫ;3:��ɫ

    light_num = shape;
    switch (color) {
        case 1:undo_light_num = 2;
            break;
        case 2:undo_light_num = (shape == 2?3:4);
            break;
        case 3:undo_light_num = 3;
            break;
    }
}

int get_light_num(void)
{
    return light_num;
}

int get_residual_light_num(int mode)
{

    if(undo_light_num == 0) return 0;
    return mode == 1?undo_light_num--:undo_light_num;
}

void SDK_finish(void)
{
    now_SDK_num++;
    //setSDKnum(now_SDK_num);
}

//void SDK_run(car_obj *pc)
//{
//    int temp_color, temp_shape;
//    switch (now_SDK_num) {
//        case 0:                     //�ߺ���1
//            car_set_aim_input_mode(pc, INPUT_POSITION);
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 0);
//            break;
//
//        case 1:                     //�ߺ���2
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 1);
//            break;
//
//
//        case 2:
//            SDK_err = Get_camera_result(now_SDK_num, &temp_color, &temp_shape);
//            if(SDK_err == -1)now_SDK_num--;
//            else
//            {
//                now_SDK_num++;
//                color_figure_done(temp_color, temp_shape);
//            }
//            break;
//        case 3:
//            car_stop(pc);
//            break;
//
//        case 4:                     //�ߺ���3
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 2);
//            break;
//        case 5:                     //�ߺ���4
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 3);
//            break;
//
//        case 6:
//            SDK_err = Get_camera_result(now_SDK_num, &temp_color, &temp_shape);
//            if(SDK_err == -1)now_SDK_num--;
//            else
//            {
//                now_SDK_num++;
//                color_figure_done(temp_color, temp_shape);
//            }
//            break;
//        case 7:
//            car_stop(pc);
//            break;
//
//        case 8:                     //�ߺ���5
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 4);
//            break;
//
//        case 9:
//            SDK_err = Get_camera_result(now_SDK_num, &temp_color, &temp_shape);
//            if(SDK_err == -1)now_SDK_num--;
//            else
//            {
//                now_SDK_num++;
//                color_figure_done(temp_color, temp_shape);
//            }
//            break;
//        case 10:
//            car_stop(pc);
//            break;
//
//
//        case 11:                     //�ߺ���6
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 5);
//            break;
//
//        case 12:                     //�ߺ���7
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 6);
//            break;
//
//
//        case 13:
//            SDK_err = Get_camera_result(now_SDK_num, &temp_color, &temp_shape);
//            if(SDK_err == -1)now_SDK_num--;
//            else
//            {
//                now_SDK_num++;
//                color_figure_done(temp_color, temp_shape);
//            }
//            break;
//        case 14:
//            car_stop(pc);
//            break;
//
//        case 15:                     //�ߺ���8
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 7);
//            break;
//
//
//        case 16:
//            SDK_err = Get_camera_result(now_SDK_num, &temp_color, &temp_shape);
//            if(SDK_err == -1)now_SDK_num--;
//            else
//            {
//                now_SDK_num++;
//                color_figure_done(temp_color, temp_shape);
//            }
//            break;
//        case 17:
//            car_stop(pc);
//            break;
//
//        case 18:                     //�ߺ���9
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 8);
//            break;
//
//
//        case 19:
//            SDK_err = Get_camera_result(now_SDK_num, &temp_color, &temp_shape);
//            if(SDK_err == -1)now_SDK_num--;
//            else
//            {
//                now_SDK_num++;
//                color_figure_done(temp_color, temp_shape);
//            }
//            break;
//        case 20:
//            car_stop(pc);
//            break;
//
//        case 21:                     //�ߺ���10
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 9);
//            break;
//
//
//        case 22:                     //�ߺ���11
//            get_waypoint(&(pc->target_dis_x),
//                         &(pc->target_dis_y), 10);
//            break;
//        default:
//            car_stop(pc);
////            now_SDK_num = 4;
//    }


void SDK_run(car_obj *pc)
{
    int temp_color, temp_shape;
    switch (now_SDK_num)
    {
        case 0:                     //�ߺ���1
            //car_stop(pc);
            car_set_aim_input_mode(pc, INPUT_POSITION);
            get_waypoint(&(pc->target_dis_x),
                         &(pc->target_dis_y), 0);
            break;

        case 1:                     //�ߺ���2
            get_waypoint(&(pc->target_dis_x),
                         &(pc->target_dis_y), 1);
            break;
        case 2:                     //�ߺ���3
            get_waypoint(&(pc->target_dis_x),
                         &(pc->target_dis_y), 2);
            break;
        case 3:                     //�ߺ���4
            get_waypoint(&(pc->target_dis_x),
                         &(pc->target_dis_y), 3);
            break;
        case 4:
           // servo_pid_start();
            break;
        default:
            car_stop(pc);
//            now_SDK_num = 4;
    }
}